#include "continuous_linear_system.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Dense>
#include <string>
#include <limits>
#include <iostream>
namespace auto_ros
{
namespace control
{
ContinuousLinearSys::ContinuousLinearSys(uint state_dim,
										 uint control_dim, uint distur_dim, uint out_dim)
	: LinearSys(state_dim, control_dim, distur_dim, out_dim)
{
}
std::shared_ptr<DiscreteLinearSys> ContinuousLinearSys::discretization(double delta_time,
																	   std::string method, double exact_torrence)
{
	if (method == "tustin")
	{
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr = std::make_shared<DiscreteLinearSys>(
			state_dim(), control_dim(), distur_dim(), out_dim());
		Eigen::MatrixXd discrete_state_matrix;
		Eigen::MatrixXd discrete_control_matrix;
		Eigen::MatrixXd discrete_distur_matrix;
		Eigen::MatrixXd discrete_out_state_matrix;
		Eigen::MatrixXd discrete_out_distur_matrix;
		Eigen::MatrixXd temp_matrix;
		temp_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) - 0.5 * delta_time * state_matrix()).inverse();
		discrete_state_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) + 0.5 * delta_time * state_matrix()) * temp_matrix;
		discrete_control_matrix = temp_matrix * control_matrix() * delta_time;
		discrete_distur_matrix = temp_matrix * distur_matrix() * delta_time;
		discrete_out_state_matrix = out_state_matrix();
		discrete_out_distur_matrix = out_distur_matrix();
		dis_sys_ptr->set_state_matrix(discrete_state_matrix);
		dis_sys_ptr->set_control_matrix(discrete_control_matrix);
		dis_sys_ptr->set_distur_matrix(discrete_distur_matrix);
		dis_sys_ptr->set_out_state_matrix(discrete_out_state_matrix);
		dis_sys_ptr->set_out_distur_matrix(discrete_out_distur_matrix);
		return dis_sys_ptr;
	}
	else if (method == "eular")
	{
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr = std::make_shared<DiscreteLinearSys>(
			state_dim(), control_dim(), distur_dim(), out_dim());
		Eigen::MatrixXd discrete_state_matrix;
		Eigen::MatrixXd discrete_control_matrix;
		Eigen::MatrixXd discrete_distur_matrix;
		Eigen::MatrixXd discrete_out_state_matrix;
		Eigen::MatrixXd discrete_out_distur_matrix;
		discrete_state_matrix = Eigen::MatrixXd::Identity(state_dim_, state_dim_) + (state_matrix() * delta_time); //.exp();
		discrete_control_matrix = delta_time * control_matrix();
		discrete_distur_matrix = delta_time * distur_matrix();
		discrete_out_state_matrix = out_state_matrix();
		discrete_out_distur_matrix = out_distur_matrix();
		dis_sys_ptr->set_state_matrix(discrete_state_matrix);
		dis_sys_ptr->set_control_matrix(discrete_control_matrix);
		dis_sys_ptr->set_distur_matrix(discrete_distur_matrix);
		dis_sys_ptr->set_out_state_matrix(discrete_out_state_matrix);
		dis_sys_ptr->set_out_distur_matrix(discrete_out_distur_matrix);
		return dis_sys_ptr;
	}
}
bool ContinuousLinearSys::discrete(std::shared_ptr<DiscreteLinearSys> dis_sys,
								   const double delta_time, const std::string method, double exact_torrence)
{
	if (method == "tustin")
	{
		Eigen::MatrixXd discrete_state_matrix;
		Eigen::MatrixXd discrete_control_matrix;
		Eigen::MatrixXd discrete_distur_matrix;
		Eigen::MatrixXd discrete_out_state_matrix;
		Eigen::MatrixXd discrete_out_distur_matrix;

		Eigen::MatrixXd temp_matrix;
		temp_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) - 0.5 * delta_time * state_matrix()).inverse();
		discrete_state_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) + 0.5 * delta_time * state_matrix()) * temp_matrix;
		discrete_control_matrix = temp_matrix * control_matrix() * delta_time;
		discrete_distur_matrix = temp_matrix * distur_matrix() * delta_time;
		discrete_out_state_matrix = out_state_matrix();
		discrete_out_distur_matrix = out_distur_matrix();

		if (!dis_sys->set_state_matrix(discrete_state_matrix))
			return false;
		if (!dis_sys->set_control_matrix(discrete_control_matrix))
			return false;
		if (!dis_sys->set_distur_matrix(discrete_distur_matrix))
			return false;
		if (!dis_sys->set_out_state_matrix(discrete_out_state_matrix))
			return false;
		if (!dis_sys->set_out_distur_matrix(discrete_out_distur_matrix))
			return false;
		return true;
	}
	else if (method == "eular")
	{
		Eigen::MatrixXd discrete_state_matrix;
		Eigen::MatrixXd discrete_control_matrix;
		Eigen::MatrixXd discrete_distur_matrix;
		Eigen::MatrixXd discrete_out_state_matrix;
		Eigen::MatrixXd discrete_out_distur_matrix;

		discrete_state_matrix = Eigen::MatrixXd::Identity(state_dim_, state_dim_) + (state_matrix() * delta_time); //.exp();
		discrete_control_matrix = delta_time * control_matrix();
		discrete_distur_matrix = delta_time * distur_matrix();
		discrete_out_state_matrix = out_state_matrix();
		discrete_out_distur_matrix = out_distur_matrix();
		if (!dis_sys->set_state_matrix(discrete_state_matrix))
			return false;
		if (!dis_sys->set_control_matrix(discrete_control_matrix))
			return false;
		if (!dis_sys->set_distur_matrix(discrete_distur_matrix))
			return false;
		if (!dis_sys->set_out_state_matrix(discrete_out_state_matrix))
			return false;
		if (!dis_sys->set_out_distur_matrix(discrete_out_distur_matrix))
			return false;
		return true;
	}
	return false;
}
bool ContinuousLinearSys::integral(
	const uint segment, const double start_time, const double end_time,
	const Eigen::VectorXd x0, const Eigen::VectorXd u, Eigen::VectorXd &xf, std::string method) //integral sys without distur
{
	if (distur_dim_ > 0)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,call wrong fun which without distur  \033[0m" << std::endl;
		return false;
	}
	if (x0.rows() != state_dim_)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,x0.rows() != state_dim_  \033[0m" << std::endl;
		return false;
	}
	else if (u.rows() != control_dim_)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,u.rows() != control_dim_ \033[0m" << std::endl;
		return false;
	}

	double delta_time = (end_time - start_time) / segment;

	Eigen::MatrixXd discrete_state_matrix;
	Eigen::MatrixXd discrete_control_matrix;
	if (method == "tustin")
	{
		Eigen::MatrixXd temp_matrix;
		temp_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) - 0.5 * delta_time * state_matrix()).inverse();
		discrete_state_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) + 0.5 * delta_time * state_matrix()) * temp_matrix;
		discrete_control_matrix = temp_matrix * control_matrix() * delta_time;
	}
	else
	{
		discrete_state_matrix = Eigen::MatrixXd::Identity(state_dim_, state_dim_) + (state_matrix() * delta_time); //.exp();
		discrete_control_matrix = delta_time * control_matrix();
	}

	xf = x0;
	for (int i = 0; i < segment; i++)
	{
		//xf = discrete_state_matrix * xf + discrete_control_matrix * u + discrete_distur_matrix * distur;
		xf = discrete_state_matrix * xf + discrete_control_matrix * u;
	}
	return true;
}
bool ContinuousLinearSys::integral(
	const uint segment, const double start_time, const double end_time,
	const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd &xf,
	std::string method) //integral sys with distur
{
	if (distur_dim_ <= 0)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,call wrong fun which with distur  \033[0m" << std::endl;
		return false;
	}
	if (x0.rows() != state_dim_)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,x0.rows() != state_dim_  \033[0m" << std::endl;
		return false;
	}
	if (distur.rows() != distur_dim_)
	{
		std::cerr << "\033[31m ContinuousLinearSys::integral failed,distur.rows() != distur_dim_ \033[0m" << std::endl;
		return false;
	}

	double delta_time = (end_time - start_time) / segment;
	Eigen::MatrixXd discrete_state_matrix;
	Eigen::MatrixXd discrete_control_matrix;
	Eigen::MatrixXd discrete_distur_matrix;
	if (method == "tustin")
	{
		Eigen::MatrixXd temp_matrix;
		temp_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) - 0.5 * delta_time * state_matrix()).inverse();
		discrete_state_matrix = (Eigen::MatrixXd::Identity(state_dim_, state_dim_) + 0.5 * delta_time * state_matrix()) * temp_matrix;
		discrete_control_matrix = temp_matrix * control_matrix() * delta_time;
		discrete_distur_matrix = temp_matrix * distur_matrix() * delta_time;
	}
	else

	{
		discrete_state_matrix = Eigen::MatrixXd::Identity(state_dim_, state_dim_) + (state_matrix() * delta_time); //.exp();
		discrete_control_matrix = delta_time * control_matrix();
		discrete_distur_matrix = delta_time * distur_matrix();
	}

	xf = x0;
	for (int i = 0; i < segment; i++)
	{

		xf = discrete_state_matrix * xf + discrete_control_matrix * u + discrete_distur_matrix * distur;

		//xf = discrete_state_matrix * xf + discrete_control_matrix * u;
	}

	return true;
}
bool ContinuousLinearSys::calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)
{
	//ny =1 solve Ax = b
	if ((state_dim_ + out_dim_) != (state_dim_ + control_dim_))
	{
		std::cout << "calc_steady_state_ctr failed,(state_dim_ + out_dim_) != (state_dim_ + control_dim_)" << std::endl;
		return false;
	}
	if (y_controlled_ref.rows() != out_dim_)
	{
		std::cout << "calc_steady_state_ctr failed,y_controlled_ref.rows() ! = out_dim_" << std::endl;
		return false;
	}

	Eigen::MatrixXd expand_matrix = Eigen::MatrixXd::Zero(state_dim_ + out_dim_, state_dim_ + control_dim_);
	Eigen::VectorXd b_vector = Eigen::VectorXd::Zero(state_dim_ + out_dim_);

	expand_matrix.block(0, 0, state_dim_, state_dim_) = state_matrix_;
	expand_matrix.block(0, state_dim_, state_dim_, control_dim_) = control_matrix_;
	expand_matrix.block(state_dim_, 0, out_dim_, state_dim_) = out_state_matrix_;
	/*std::cout << "expand matrix is:" << std::endl
			  << expand_matrix << std::endl;
	std::cout << "expand_matrix inv is:" << std::endl
			  << expand_matrix.inverse() << std::endl;*/
	b_vector.block(0, 0, state_dim_, 1) = -distur_matrix_ * distur_ref;
	/*std::cout << "-distur_matrix_ * distur_ref is:" << std::endl
			  << -distur_matrix_ * distur_ref << std::endl;*/

	if (out_distur_matrix_.rows() != 0)
	{
		b_vector.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref - out_distur_matrix_ * distur_ref;
	}
	else
	{
		b_vector.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref;
	}

	Eigen::VectorXd steady_state_ctr_vector = expand_matrix.inverse() * b_vector;
	steady_state = steady_state_ctr_vector.block(0, 0, state_dim_, 1);
	steady_control = steady_state_ctr_vector.block(state_dim_, 0, control_dim_, 1);
	return true;
}

} // namespace control
} // namespace auto_ros